<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of gspf</title>
  <meta name="keywords" content="gspf">
  <meta name="description" content="GSPF  Gaussian Sum Particle Filter">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; gspf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>gspf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>GSPF  Gaussian Sum Particle Filter</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [estimate, ParticleFilterDS, pNoise, oNoise] = gspf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> GSPF  Gaussian Sum Particle Filter

   This is an implementation of Jayesh H. Kotecha and Petar M. Djuric's
   'Gaussian Sum Particle Filter' as presented in:

   Jayesh H. Kotecha and Petar M. Djuric, &quot;Gaussian Sum Particle
   Filtering for Dynamic State Space Models&quot;, Proceedings of ICASSP-2001,
   Salt Lake City, Utah, May 2001.

   [estimate, ParticleFilterDS, pNoise, oNoise] = GSPF(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         ParticleFilterDS     Particle filter data structure. (see field definitions below)
         pNoise               (NoiseDS) process noise data structure  (must be of type 'gmm')
         oNoise               (NoiseDS) observation noise data structure
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          Inference data structure generated by GENINFDS function.

   OUTPUT
         estimate             State estimate generated from posterior distribution of state given all observation. Type of
                              estimate is specified by InferenceDS.estimateType
         ParticleFilterDS     Updated Particle filter data structure.
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

   ParticleFilterDS fields:
         .N                   (scalar) number of particles to use
         .stateGMM            (gmm) Gaussian mixture model of state distribution with the following field:
                  .M            (scalar) number of mixture components in GMM
                  .mu           (statedim-by-M) buffer of mean vectors (centroids) of state GMM components
                  .cov          (statedim-by-statedim-my-M) buffer of covariance matrices of state GMM components
                  .cov_type     (string) covariance matrix type ('full','sqrt','diag','swrt-diag') 'sqrt' is preferred.
                  .weights      (1-by-M) state GMM component weights (priors)


   Required InferenceDS fields:
         .estimateType        (string) Estimate type : 'mean', 'mode', etc.

   NOTE : All covariances are assumed to be of type 'sqrt', i.e. Cholesky factors.

   See also
   <a href="pf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise] = pf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">PF</a>, <a href="sppf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise] = sppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">SPPF</a>
   Copyright (c) Oregon Health &amp; Science University (2006)

   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
   academic use only (see included license file) and can be obtained from
   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
   software should contact rebel@csee.ogi.edu for commercial licensing information.

   See LICENSE (which should be part of the main toolkit distribution) for more
   detail.</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="residualresample.html" class="code" title="function outIndex = residualResample(inIndex, weights);">residualresample</a>	RESIDUALRESAMPLE  Residual resampling for SIR. Performs the resampling stage of</li></ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse2.html" class="code" title="">demse2</a>	DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse5.html" class="code" title="">demse5</a>	DEMSE4  Bearing and Frequency Tracking Example</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [estimate, ParticleFilterDS, pNoise, oNoise] = gspf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% GSPF  Gaussian Sum Particle Filter</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   This is an implementation of Jayesh H. Kotecha and Petar M. Djuric's</span>
0006 <span class="comment">%   'Gaussian Sum Particle Filter' as presented in:</span>
0007 <span class="comment">%</span>
0008 <span class="comment">%   Jayesh H. Kotecha and Petar M. Djuric, &quot;Gaussian Sum Particle</span>
0009 <span class="comment">%   Filtering for Dynamic State Space Models&quot;, Proceedings of ICASSP-2001,</span>
0010 <span class="comment">%   Salt Lake City, Utah, May 2001.</span>
0011 <span class="comment">%</span>
0012 <span class="comment">%   [estimate, ParticleFilterDS, pNoise, oNoise] = GSPF(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0013 <span class="comment">%</span>
0014 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0015 <span class="comment">%</span>
0016 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0017 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0018 <span class="comment">%</span>
0019 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state</span>
0020 <span class="comment">%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the</span>
0021 <span class="comment">%   system.</span>
0022 <span class="comment">%</span>
0023 <span class="comment">%   INPUT</span>
0024 <span class="comment">%         ParticleFilterDS     Particle filter data structure. (see field definitions below)</span>
0025 <span class="comment">%         pNoise               (NoiseDS) process noise data structure  (must be of type 'gmm')</span>
0026 <span class="comment">%         oNoise               (NoiseDS) observation noise data structure</span>
0027 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0028 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0029 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0030 <span class="comment">%         InferenceDS          Inference data structure generated by GENINFDS function.</span>
0031 <span class="comment">%</span>
0032 <span class="comment">%   OUTPUT</span>
0033 <span class="comment">%         estimate             State estimate generated from posterior distribution of state given all observation. Type of</span>
0034 <span class="comment">%                              estimate is specified by InferenceDS.estimateType</span>
0035 <span class="comment">%         ParticleFilterDS     Updated Particle filter data structure.</span>
0036 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0037 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0038 <span class="comment">%</span>
0039 <span class="comment">%   ParticleFilterDS fields:</span>
0040 <span class="comment">%         .N                   (scalar) number of particles to use</span>
0041 <span class="comment">%         .stateGMM            (gmm) Gaussian mixture model of state distribution with the following field:</span>
0042 <span class="comment">%                  .M            (scalar) number of mixture components in GMM</span>
0043 <span class="comment">%                  .mu           (statedim-by-M) buffer of mean vectors (centroids) of state GMM components</span>
0044 <span class="comment">%                  .cov          (statedim-by-statedim-my-M) buffer of covariance matrices of state GMM components</span>
0045 <span class="comment">%                  .cov_type     (string) covariance matrix type ('full','sqrt','diag','swrt-diag') 'sqrt' is preferred.</span>
0046 <span class="comment">%                  .weights      (1-by-M) state GMM component weights (priors)</span>
0047 <span class="comment">%</span>
0048 <span class="comment">%</span>
0049 <span class="comment">%   Required InferenceDS fields:</span>
0050 <span class="comment">%         .estimateType        (string) Estimate type : 'mean', 'mode', etc.</span>
0051 <span class="comment">%</span>
0052 <span class="comment">%   NOTE : All covariances are assumed to be of type 'sqrt', i.e. Cholesky factors.</span>
0053 <span class="comment">%</span>
0054 <span class="comment">%   See also</span>
0055 <span class="comment">%   PF, SPPF</span>
0056 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0057 <span class="comment">%</span>
0058 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0059 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0060 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0061 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0062 <span class="comment">%</span>
0063 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0064 <span class="comment">%   detail.</span>
0065 
0066 <span class="comment">%=============================================================================================</span>
0067 
0068 <span class="keyword">if</span> (nargin ~= 7) error(<span class="string">' [ gspf ] Not enough input arguments.'</span>); <span class="keyword">end</span>
0069 
0070 <span class="keyword">switch</span> pNoise.ns_type
0071 <span class="keyword">case</span> <span class="string">'gmm'</span>
0072 <span class="keyword">otherwise</span>
0073   error(<span class="string">' [ gspf ] Process noise source must be of type : gmm (Gaussian Mixture Model)'</span>);
0074 <span class="keyword">end</span>
0075 
0076 
0077 Xdim  = InferenceDS.statedim;                            <span class="comment">% extract state dimension</span>
0078 Odim  = InferenceDS.obsdim;                              <span class="comment">% extract observation dimension</span>
0079 U1dim = InferenceDS.U1dim;                               <span class="comment">% extract exogenous input 1 dimension</span>
0080 U2dim = InferenceDS.U2dim;                               <span class="comment">% extract exogenous input 2 dimension</span>
0081 Vdim  = InferenceDS.Vdim;                                <span class="comment">% extract process noise dimension</span>
0082 Ndim  = InferenceDS.Ndim;                                <span class="comment">% extract observation noise dimension</span>
0083 
0084 numP = ParticleFilterDS.N;            <span class="comment">% number of particles to use for SIR</span>
0085 
0086 stateGMM = ParticleFilterDS.stateGMM;
0087 
0088 G    = stateGMM.M;      <span class="comment">% number of components in state GMM</span>
0089 K    = pNoise.M;        <span class="comment">% number of components in process noise GMM</span>
0090 
0091 GK  = G*K;
0092 
0093 sampleBuf1 = zeros(Xdim,numP,G);     <span class="comment">% sample buffer : (sample dimension) X (number of samples) X (number of mixcomps)</span>
0094 sampleBuf2 = zeros(Xdim,numP,GK);
0095 stateWNew = zeros(1,GK);
0096 
0097 sampleBuf3 = zeros(Xdim,numP,GK);
0098 impWeights = zeros(GK,numP);
0099 
0100 stateMu  = stateGMM.mu;
0101 stateCov = stateGMM.cov;
0102 stateW   = stateGMM.weights;
0103 
0104 cov_type = stateGMM.cov_type;
0105 
0106 <span class="keyword">switch</span> cov_type
0107 <span class="keyword">case</span> {<span class="string">'full'</span>,<span class="string">'diag'</span>}
0108   error(<span class="string">' [ gspf ] Currently the GSPF algorithm only support state GMMs which has ''sqrt'' covariance types.'</span>);
0109 <span class="keyword">end</span>
0110 
0111 
0112 stateMuNew  = zeros(Xdim,GK);
0113 stateCovNew = zeros(Xdim,Xdim,GK);
0114 
0115 pNoiseW = pNoise.weights;
0116 
0117 ones_numP = ones(numP,1);
0118 ones_Xdim = ones(1,Xdim);
0119 ones_GK   = ones(GK,1);
0120 
0121 NOV = size(obs,2);                                       <span class="comment">% number of input vectors</span>
0122 
0123 <span class="keyword">if</span> (U1dim==0), UU1=zeros(0,numP); <span class="keyword">end</span>
0124 <span class="keyword">if</span> (U2dim==0), UU2=zeros(0,numP); <span class="keyword">end</span>
0125 
0126 estimate   = zeros(Xdim,NOV);
0127 
0128 
0129 <span class="comment">%================================================================================================</span>
0130 <span class="comment">%--- MAIN LOOP over all data vectors</span>
0131 
0132 <span class="keyword">for</span> i=1:NOV,
0133 
0134     OBStemp = obs(:,i);                <span class="comment">% inline cvecrep</span>
0135     OBS = OBStemp(:,ones_numP);
0136 
0137     <span class="keyword">if</span> U1dim
0138       Utemp = U1(:,i);
0139       UU1 = Utemp(:,ones_numP);        <span class="comment">% inline cvecrep</span>
0140     <span class="keyword">end</span>
0141     <span class="keyword">if</span> U2dim
0142       Utemp = U2(:,i);
0143       UU2 = Utemp(:,ones_numP);        <span class="comment">% inline cvecrep</span>
0144     <span class="keyword">end</span>
0145 
0146 
0147     <span class="comment">%-----------------------------------------------------------------------</span>
0148     <span class="comment">% TIME UPDATE</span>
0149 
0150     <span class="keyword">for</span> g=1:G,                                 <span class="comment">% draw M samples from each state GMM component</span>
0151         temp_mu = stateMu(:,g);
0152         <span class="comment">% It is assumed that the covariances are Cholesky factors</span>
0153         sampleBuf1(:,:,g) = stateCov(:,:,g) * randn(Xdim,numP) + temp_mu(:,ones_numP);
0154     <span class="keyword">end</span>
0155 
0156     <span class="keyword">for</span> k=1:K,
0157         cS  = pNoise.cov(:,:,k);       <span class="comment">% get process noise GMM component covariance</span>
0158         cMu = pNoise.mu(:,k);
0159         cMuBuf = cMu(:,ones_numP);
0160         <span class="keyword">for</span> g=1:G,
0161             gk = g + (k-1)*G;
0162             pNoiseBuf = cS * randn(Vdim,numP) + cMuBuf;
0163             sampleBuf2(:,:,gk) = InferenceDS.ffun( InferenceDS, sampleBuf1(:,:,g), pNoiseBuf, UU1);
0164             stateWNew(1,gk) = stateW(1,g) * pNoiseW(1,k);
0165         <span class="keyword">end</span>
0166     <span class="keyword">end</span>
0167 
0168     stateWNew = stateWNew / sum(stateWNew);
0169 
0170     <span class="keyword">for</span> gk=1:GK,                                    <span class="comment">% inline sample mean and covariance</span>
0171         muFoo = sum(sampleBuf2(:,:,gk),2)/numP;
0172         stateMuNew(:,gk) = muFoo;
0173         muFoo = muFoo(:,ones_numP);
0174         Xfoo = sampleBuf2(:,:,gk) - muFoo;
0175         [foo,covFoo] = qr(Xfoo',0);
0176         stateCovNew(:,:,gk) = covFoo'/sqrt(numP-1);
0177     <span class="keyword">end</span>
0178 
0179 
0180     <span class="comment">%-----------------------------------------------------------------------</span>
0181     <span class="comment">% MEASUREMENT UPDATE</span>
0182 
0183 
0184     <span class="comment">% Calculate observed samples and importance weights</span>
0185     <span class="keyword">for</span> gk=1:GK,
0186         temp_mu = stateMuNew(:,gk);
0187         sampleBuf3(:,:,gk) = stateCovNew(:,:,gk) * randn(Xdim,numP) + temp_mu(:,ones_numP);
0188         impWeights(gk,:) = InferenceDS.likelihood( InferenceDS, OBS, sampleBuf3(:,:,gk), UU2, oNoise) + 1e-99;
0189     <span class="keyword">end</span>
0190 
0191     weightNorm = 0;
0192 
0193     <span class="comment">% Calculate updated state mixcomp means, covariances and weights</span>
0194     <span class="keyword">for</span> gk=1:GK,
0195 
0196         weightFoo = impWeights(gk,:);
0197         impWeightM = weightFoo(ones_Xdim,:);   <span class="comment">% inline rvecrep</span>
0198         impWeightNorm = sum(weightFoo);
0199         muFoo2 = sum(impWeightM.*sampleBuf3(:,:,gk),2) / impWeightNorm;
0200         stateMuNew(:,gk) = muFoo2;
0201 
0202         xdel = sampleBuf3(:,:,gk) - muFoo2(:,ones_numP); <span class="comment">% inline cvecrep</span>
0203         weightSFoo = sqrt(weightFoo);
0204         impWeightSM = weightSFoo(ones_Xdim,:);
0205 
0206         Xfoo = impWeightSM.*xdel;
0207 
0208         [foo,covFoo] = qr(Xfoo',0);
0209         stateCovNew(:,:,gk) = covFoo'/sqrt(impWeightNorm);
0210 
0211         stateWNew(:,gk) = stateWNew(:,gk)*impWeightNorm;   <span class="comment">% part 1 of equation (11)</span>
0212         weightNorm = weightNorm + impWeightNorm;
0213 
0214     <span class="keyword">end</span>
0215 
0216     <span class="comment">% Calculate updated and normalized mixcomp weights</span>
0217     stateWNew = stateWNew / weightNorm;                   <span class="comment">% part 2 of equation (11)</span>
0218     stateWNew = stateWNew / sum(stateWNew);               <span class="comment">% normalize</span>
0219 
0220 
0221     <span class="comment">%-----------------------------------------------------------------------</span>
0222     <span class="comment">% CALCULATE ESTIMATE</span>
0223 
0224     <span class="keyword">switch</span> InferenceDS.estimateType
0225 
0226     <span class="keyword">case</span> <span class="string">'mean'</span>
0227         muFoo = sum(stateWNew(ones_Xdim,:).*stateMuNew,2);
0228         estimate(:,i) = muFoo;
0229 
0230     <span class="keyword">otherwise</span>
0231         error(<span class="string">' [ sppf ] Unknown estimate type.'</span>);
0232 
0233     <span class="keyword">end</span>
0234 
0235 
0236     <span class="comment">%-----------------------------------------------------------------------</span>
0237     <span class="comment">% RESAMPLE MIXTURE COMPONENTS</span>
0238 
0239     resampleIdx = <a href="residualresample.html" class="code" title="function outIndex = residualResample(inIndex, weights);">residualresample</a>(1:GK,stateWNew);
0240     [fooIdx,rIdx]=sort(rand(1,GK));                  <span class="comment">% inline randperm</span>
0241     rIdx=rIdx(1:G);
0242 
0243     idx = resampleIdx(rIdx);
0244     stateMu = stateMuNew(:,idx);
0245     stateCov = stateCovNew(:,:,idx);
0246 
0247     stateW = (1/G) * ones(1,G);
0248 
0249     <span class="keyword">if</span> pNoise.adaptMethod
0250         error(<span class="string">'  [ gspf ] Process noise adaptation not supported yet for GMM noise sources.'</span>);
0251     <span class="keyword">end</span>
0252 
0253 
0254 <span class="comment">%--------------------------------------------------------------------------</span>
0255 <span class="keyword">end</span>     <span class="comment">%.. loop over input vectors</span>
0256 
0257 
0258 ParticleFilterDS.stateGMM.cov      = stateCov;
0259 ParticleFilterDS.stateGMM.mu       = stateMu;
0260 ParticleFilterDS.stateGMM.weights  = stateW;
0261</pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>